#include "EnemyAI.h"

namespace PeanutQuest {

	EnemyAI::EnemyAI() {
		_axesScale = 1.0;
		_position.set(50,0,0);
		_orientation.setRotationDeg(0,cg::Vector3d(0,1,0));
		_front.set(1,0,0);
		_up.set(0,1,0);
		_right.set(0,0,1);
		_linearVelocity = 0;
		_angularVelocity = 0;
		_isGoAhead = false;
		_isGoBack = false;
		_isYawLeft = false;
		_isYawRight = false;
		_isPitchUp = false;
		_isPitchDown = false;
		_isRollLeft = false;
		_isRollRight = false;
		bool left = true;
		bool right = false;
		temp = getPosition().data[0];
	}
	EnemyAI::~EnemyAI() {
	}
	cg::Vector3d EnemyAI::getPosition() {	return _position; }
	cg::Quaterniond EnemyAI::getOrientation() { return _orientation; }
	cg::Vector3d EnemyAI::getFront() { return _front; }
	cg::Vector3d EnemyAI::getUp() { return _up; }
	cg::Vector3d EnemyAI::getRight() { return _right; }

	void EnemyAI::setPosition(double x, double y, double z) {
		_position.set(x,y,z);
	}
	void EnemyAI::setLinearVelocity(double value) {
		_linearVelocity = value;
	}
	void EnemyAI::setAngularVelocity(double value) {
		_angularVelocity = value;
	}
	void EnemyAI::goAhead() { _isGoAhead = true; }
	void EnemyAI::goBack() { _isGoBack = true; }
	void EnemyAI::yawLeft() { _isYawLeft = true; }
	void EnemyAI::yawRight() { _isYawRight = true; }
	void EnemyAI::pitchUp() { _isPitchUp = true; }
	void EnemyAI::pitchDown() { _isPitchDown = true; }
	void EnemyAI::rollLeft() { _isRollLeft = true; }
	void EnemyAI::rollRight() { _isRollRight = true; }

	inline
	void EnemyAI::rotate(double elapsed_seconds, int direction, 
						   cg::Vector3d axis, cg::Vector3d& v1, cg::Vector3d& v2) 
	{
		_q.setRotationDeg(direction * _angularVelocity * elapsed_seconds,axis);
		v1 = apply(_q,v1);
		v2 = apply(_q,v2);
		_orientation = _q * _orientation;
	}
	void EnemyAI::step(double elapsed_seconds) {
		if(_isGoAhead) {
			_position += _front * _linearVelocity * elapsed_seconds;
			_isGoAhead = false;
		}
		if(_isGoBack) {
			_position -= _front * _linearVelocity * elapsed_seconds;
			_isGoBack = false;
		}
		if(_isYawLeft) {
			rotate(elapsed_seconds,1,_up,_front,_right);
			_isYawLeft = false;
		}
		if(_isYawRight) {
			rotate(elapsed_seconds,-1,_up,_front,_right);
			_isYawRight = false;
		}
		if(_isPitchUp) {
			rotate(elapsed_seconds,1,_right,_up,_front);
			_isPitchUp = false;
		}
		if(_isPitchDown) {
			rotate(elapsed_seconds,-1,_right,_up,_front);
			_isPitchDown = false;
		}
		if(_isRollLeft) {
			rotate(elapsed_seconds,1,_front,_right,_up);
			_isRollLeft = false;
		}
		if(_isRollRight) {
			rotate(elapsed_seconds,-1,_front,_right,_up);
			_isRollRight = false;
		}
		_orientation.getGLMatrix(_rotationMatrix);
	}
	void EnemyAI::applyTransforms() {
        glTranslated(_position[0],_position[1],_position[2]);
		glMultMatrixd(_rotationMatrix);
	}
	void EnemyAI::setAxesScale(double scale) {
		_axesScale = scale;
	}
	void EnemyAI::drawAxes() {
		glPushMatrix();
			glTranslated(_position[0],_position[1],_position[2]);
			glScaled(_axesScale,_axesScale,_axesScale);
			glColor3f(1.0f,0.0f,0.0f);
			glBegin(GL_LINES);
				glVertex3f(0.0f,0.0f,0.0f);
				glVertex3d(_front[0],_front[1],_front[2]);
			glEnd();
			glColor3f(0.0f,1.0f,0.0f);
			glBegin(GL_LINES);
				glVertex3d(0.0,0.0,0.0);
				glVertex3d(_up[0],_up[1],_up[2]);
			glEnd();
			glColor3f(0,0,1);
			glBegin(GL_LINES);
				glVertex3d(0.0,0.0,0.0);
				glVertex3d(_right[0],_right[1],_right[2]);
			glEnd();
		glPopMatrix();
	}

	void EnemyAI::aiAnimation(double elapsed_seconds) {

		if(left) {
			this->goAhead();
			if(getPosition().data[0] > temp + 5) {
				left = false;
				right = true;
			}
		}

		else if(right)
			this->goBack();
			if(getPosition().data[0] < temp - 5) {
				left = true;
				right = false;
			}

		this->step(elapsed_seconds); 
	}
}